//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <iostream>
#include <vector>
#include <sys/types.h>
#include <dirent.h>
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <cmath>
#include <cstring>
#include <unistd.h>
#include "lvx_file.h"
#include "third_party/rapidxml/rapidxml.hpp"
#include "third_party/rapidxml/rapidxml_utils.hpp"
#include <sys/stat.h>
#include <string.h>

#define WRITE_BUFFER_LEN 1024 * 1024
#define MAGIC_CODE       (0xac0ea767)
#define RAW_POINT_NUM     100
#define SINGLE_POINT_NUM  96
#define DUAL_POINT_NUM    48
#define TRIPLE_POINT_NUM  30
#define IMU_POINT_NUM     1
#define M_PI             3.14159265358979323846
#define staticFileNameLength 30

//using namespace std;

//string va;
//add 2021-12-22
int saveFileName(char *filename){
	
	time_t curtime = time(NULL);
  	//char filename[30] = { 0 };
	char str_dir[128] = {0};

  	struct tm* local_time = localtime(&curtime);
  	strftime(filename, 128, "%Y%m%d", local_time);
    srand((unsigned int)curtime);
	//printf("%d %s\n ",sizeof(filename),filename);
	sprintf(str_dir,"/media/ubuntu/bae/%s%05d",filename,rand()%100000);
	mkdir(str_dir,0007);
  	FILE *fp;
  	fp = fopen("filenameTemp.txt","wb");
  	while(fp==NULL){
  		usleep(30);
  		fp = fopen("filenameTemp.txt","wb");
  	} 
  	fwrite(filename,strlen(filename),1,fp);
  	fwrite("\n",1,1,fp);
  	fclose(fp);
  	return 1;
}
int getFileName(char *filename){
	
	FILE *fp;
	fp = fopen("filenameTemp.txt","rb+");
  	while(fp==NULL){
  		usleep(30);
  		fp = fopen("filenameTemp.txt","rb+");
  	} 
  	
	fscanf(fp,"%[^\n]", filename);
	
	
	printf("filename %s\n",filename);
	fclose(fp);
	return 1;;
}



LvxFileHandle * g_lxvFilePtr = NULL;


LvxFileHandle::LvxFileHandle() : cur_frame_index_(0), cur_offset_(0), frame_duration_(kDefaultFrameDurationTime) {

    g_lxvFilePtr = NULL;
}

bool LvxFileHandle::InitLvxFile() {
  time_t curtime = time(nullptr);
  char filename[128] = { 0 };
  
  saveFileName(filename);
 
  getFileName(filename);

  char path[100] = { 0 };

  //printf("------------%s",va.c_str());
  //sprintf(path,"%s/%s",va.c_str(),filename);
  sprintf(path,"/media/ubuntu/bae/%s/lvx",filename);
  printf("path %s\n",path);
  
  lvx_file_.open(path, std::ios::out | std::ios::binary);
	
	
  if (!lvx_file_.is_open()) {
    return false;
  }
  g_lxvFilePtr = this;
  return true;
}

void LvxFileHandle::InitLvxFileHeader() {
  LvxFilePublicHeader lvx_file_public_header = { 0 };
  std::unique_ptr<char[]> write_buffer(new char[WRITE_BUFFER_LEN]);
  std::string signature = "livox_tech";
  memcpy(lvx_file_public_header.signature, signature.c_str(), signature.size());

  lvx_file_public_header.version[0] = 1;
  lvx_file_public_header.version[1] = 1;
  lvx_file_public_header.version[2] = 0;
  lvx_file_public_header.version[3] = 0;

  lvx_file_public_header.magic_code = MAGIC_CODE;

  memcpy(write_buffer.get() + cur_offset_, (void *)&lvx_file_public_header, sizeof(LvxFilePublicHeader));
  cur_offset_ += sizeof(LvxFilePublicHeader);

  uint8_t device_count = static_cast<uint8_t>(device_info_list_.size());
  LvxFilePrivateHeader lvx_file_private_header = { 0 };
  lvx_file_private_header.frame_duration = frame_duration_;
  lvx_file_private_header.device_count = device_count;

  memcpy(write_buffer.get() + cur_offset_, (void *)&lvx_file_private_header, sizeof(LvxFilePrivateHeader));
  cur_offset_ += sizeof(LvxFilePrivateHeader);

  for (int i = 0; i < device_count; i++) {
    memcpy(write_buffer.get() + cur_offset_, (void *)&device_info_list_[i], sizeof(LvxDeviceInfo));
    cur_offset_ += sizeof(LvxDeviceInfo);
  }

  lvx_file_.write((char *)write_buffer.get(), cur_offset_);
}




void LvxFileHandle::lenToHex(char *des,long len){
   

     des[0] = len&0x00ff;
     des[1] = (len>>8)&0x00ff;
     des[2] = (len>>16)&0x00ff;
     des[3] = (len>>24)&0x00ff;
    
}



void LvxFileHandle::SaveFrameToLvxFile(std::list<LvxBasePackDetail> &point_packet_list_temp) {
  //before saveFrame ,add package header 
  Package pack = {0};
  pack.header = 0x7b;
  pack.kind = 0x03;

  char filename[staticFileNameLength] = {0};
  getFileName(filename);
  char path[18+staticFileNameLength] = { 0 };

  //sprintf(path,"%s/%s",va.c_str(),filename);
  //sprintf(path,"/media/ubuntu/bae/%s/lvx",filename);

  //lvx_file_.open((const char*)path,std::ios::app | std::ios::binary);
  
 // while(!lvx_file_.is_open()){
 //   usleep(30);
 //   lvx_file_.open((const char*)path,std::ios::app | std::ios::binary);
 // }

  uint64_t cur_pos = 0;
  uint64_t frame_length = 0;
  //FrameHeader frame_header = { 0 };
  std::unique_ptr<char[]> write_buffer(new char[WRITE_BUFFER_LEN]);

  /*frame_header.current_offset = cur_offset_;
  frame_header.next_offset = cur_offset_ + sizeof(FrameHeader) + sizeof(pack.header)*8;*/
  auto iterator = point_packet_list_temp.begin();
  for (; iterator != point_packet_list_temp.end(); iterator++) {
	frame_length += iterator -> pack_size;
    //frame_header.next_offset += iterator->pack_size;
  }

  //frame_header.frame_index = cur_frame_index_;
  //add head
  memcpy(write_buffer.get()+cur_pos,&pack.header,sizeof(pack.header));
  cur_pos += sizeof(pack.header);
  memcpy(write_buffer.get()+cur_pos,&pack.kind,sizeof(pack.kind));
  cur_pos += sizeof(pack.kind);
  
  //add length
  //frame_length = frame_header.next_offset - frame_header.current_offset - 8*sizeof(char);
  char des[4];
  lenToHex(des,frame_length);
  pack.length_07 = des[0];
  pack.length_815 = des[1];
  pack.length_1623 = des[2];
  pack.length_2431 = des[3];
  memcpy(write_buffer.get()+cur_pos,&pack.length_07,sizeof(pack.kind));
  cur_pos += sizeof(pack.kind);
  memcpy(write_buffer.get()+cur_pos,&pack.length_815,sizeof(pack.kind));
  cur_pos += sizeof(pack.kind);
  memcpy(write_buffer.get()+cur_pos,&pack.length_1623,sizeof(pack.kind));
  cur_pos += sizeof(pack.kind);
  memcpy(write_buffer.get()+cur_pos,&pack.length_2431,sizeof(pack.kind));
  cur_pos += sizeof(pack.kind);
  
  //memcpy(write_buffer.get() + cur_pos, (void*)&frame_header, sizeof(FrameHeader));
  //cur_pos += sizeof(FrameHeader);
  
  
  pthread_mutex_lock(&mutex_file_wr);
  auto iter = point_packet_list_temp.begin();
  for (; iter != point_packet_list_temp.end(); iter++) {
    if (cur_pos + iter->pack_size >= WRITE_BUFFER_LEN) {
      lvx_file_.write((char*)write_buffer.get(), cur_pos);
      cur_pos = 0;
      memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), iter->pack_size);
      cur_pos += iter->pack_size;
    }
    else {
      memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), iter->pack_size);
      cur_pos += iter->pack_size;
    }
  }

  lvx_file_.write((char*)write_buffer.get(), cur_pos);
  //add tail 
  pack.tail1 = 0x0d;
  pack.tail2 = 0x0a;
  lvx_file_.write((char*)&pack.tail1, 1);
  lvx_file_.write((char*)&pack.tail2, 1);
  lvx_file_.flush();
  pthread_mutex_unlock(&mutex_file_wr); 
  //lvx_file_.close();
  

  //cur_offset_ = frame_header.next_offset;
  //cur_frame_index_++;	
	

}

void LvxFileHandle::CloseLvxFile() {
  if (lvx_file_.is_open())
    lvx_file_.close();
}

void LvxFileHandle::write_data(unsigned char * buff, long len,int flush_flag)
{
  if(lvx_file_.is_open())
  {
    lvx_file_.write((char*)buff,len);
    if(flush_flag)
    {
       lvx_file_.flush();
    }
  }
}



void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data, LvxBasePackDetail &packet) {
  packet.version = data->version;
  packet.port_id = data->slot;
  packet.lidar_index = data->id;
  packet.rsvd = data->rsvd;
  packet.error_code = data->err_code;
  packet.timestamp_type = data->timestamp_type;
  packet.data_type = data->data_type;
  memcpy(packet.timestamp, data->timestamp, 8 * sizeof(uint8_t));
  switch (packet.data_type) {
    case PointDataType::kCartesian:
       packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point) - \
          sizeof(packet.pack_size) + RAW_POINT_NUM*sizeof(LivoxRawPoint);
      memcpy(packet.raw_point,(void *)data->data, RAW_POINT_NUM*sizeof(LivoxRawPoint));
      break;
    case PointDataType::kSpherical :
      packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +RAW_POINT_NUM*sizeof(LivoxSpherPoint);
      memcpy(packet.raw_point,(void *)data->data, RAW_POINT_NUM*sizeof(LivoxSpherPoint));
      break;
    case PointDataType::kExtendCartesian :
      packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint);
      memcpy(packet.raw_point,(void *)data->data, SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint));
      break;
    case PointDataType::kExtendSpherical :
      packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint);
      memcpy(packet.raw_point,(void *)data->data, SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint));
      break;
    case PointDataType::kDualExtendCartesian :
      packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint);
      memcpy(packet.raw_point,(void *)data->data, DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint));
      break;
    case PointDataType::kDualExtendSpherical :
      packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint);
      memcpy(packet.raw_point,(void *)data->data, DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint));
      break;
    case PointDataType::kImu :
      packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +IMU_POINT_NUM*sizeof(LivoxImuPoint);
      memcpy(packet.raw_point,(void *)data->data, IMU_POINT_NUM*sizeof(LivoxImuPoint));
      break;
    case PointDataType::kTripleExtendCartesian :
      packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendRawPoint);
      memcpy(packet.raw_point,(void *)data->data, TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendRawPoint));
      break;
    case PointDataType::kTripleExtendSpherical :
      packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendSpherPoint);
      memcpy(packet.raw_point,(void *)data->data, TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendSpherPoint));
      break;
    default:
      break;
  }
}

void ParseExtrinsicXml(DeviceItem &item, LvxDeviceInfo &info) {
  rapidxml::file<> extrinsic_param("extrinsic.xml");
  rapidxml::xml_document<> doc;
  doc.parse<0>(extrinsic_param.data());
  rapidxml::xml_node<>* root = doc.first_node();
  if ("Livox" == (std::string)root->name()) {
    for (rapidxml::xml_node<>* device = root->first_node(); device; device = device->next_sibling()) {
      if ("Device" == (std::string)device->name() && (strncmp(item.info.broadcast_code, device->value(), kBroadcastCodeSize) == 0)) {
        memcpy(info.lidar_broadcast_code, device->value(), kBroadcastCodeSize);
        memset(info.hub_broadcast_code, 0, kBroadcastCodeSize);
        info.device_type = item.info.type;
        info.device_index = item.handle;
        for (rapidxml::xml_attribute<>* param = device->first_attribute(); param; param = param->next_attribute()) {
          if ("roll" == (std::string)param->name()) info.roll = static_cast<float>(atof(param->value()));
          if ("pitch" == (std::string)param->name()) info.pitch = static_cast<float>(atof(param->value()));
          if ("yaw" == (std::string)param->name()) info.yaw = static_cast<float>(atof(param->value()));
          if ("x" == (std::string)param->name()) info.x = static_cast<float>(atof(param->value()));
          if ("y" == (std::string)param->name()) info.y = static_cast<float>(atof(param->value()));
          if ("z" == (std::string)param->name()) info.z = static_cast<float>(atof(param->value()));
        }
      }
    }
  }
}


void write_to_lvx(unsigned char * buff, long len, int flush_flag)
{
    if(g_lxvFilePtr)
    {
       g_lxvFilePtr->write_data(buff,len,flush_flag);
    }
}

pthread_mutex_t mutex_file_wr = PTHREAD_MUTEX_INITIALIZER;
